#include "demo.h"
#include "debug.h"
#include "tool.h"
#include "get_omega.h"

String videoPath0 = "video/14.avi";


//相机
#ifdef DH_OPEN_CAMERA
	DHCamera cap;
#endif

#ifdef KSJ_OPEN_CAMERA
	KSJCamera cap(GWIDTH, GHEIGHT, 1);//728 544
#endif

#ifdef DV_OPEN_CAMERA
	DvpCamera_class cap(FRAMESIZE, EXPOSURETIME, FRAMEGAIN);
#endif

#ifdef OPEN_CAMERA
	VideoCapture cap(videoPath0);
#endif


//自瞄公共变量

Rect RoiRect; 	                    //感兴趣区域
uchar ReceiveBuffer[1] = { '0' };   //接收到的字符
vector<float> vdistance;            //距离容器-用于数据滤波
int unfind = 0;                     //未找到的帧数
Gyro gyro;                          //小陀螺对象
char c = 0;                         //字符变量-用于测试装甲识别和能量机关模式转换
int fire = 0;			    


//能量机关变量

int g_num = 0;
int g_buff_type = 0;
int g_show_num = 0;
int g_target_num = 0;
int g_direction = 0;
int g_color = 0;

float g_time0 = 0, g_time1 = 0;
float g_angle1 = 0;
float g_d_angle = 0;

bool statr = false;
double Sum = 0;
int Sum_count = 0;


pthread_mutex_t xmlfile_mutex;  //自瞄读取图像
pthread_mutex_t cope_mutex;     //自瞄图像处理
pthread_mutex_t srcimag_mutex;  //自瞄读取图像

pthread_mutex_t mutex1;
pthread_mutex_t mutex2;
pthread_mutex_t mutex3;
pthread_mutex_t mutex4;



void* Run1(void* arg)
{
	while(1)
	{
		ArmorRecognition Target(g_fs_read); //识别装甲板对象声明
		SendData uart;                      //串口对象声明
		Mat srcimage;                       //相机获取的原图像
		Mat Roi_image;                      //实际处理的roi图像
		int writeCount;                     //保存的比赛图片数
		
		RoiRect = Rect(0, 0, GWIDTH, GHEIGHT);
	
		while(1)
		{
			//通过接收控制组发的字符，进入相应模式
			if (ReceiveBuffer[0] == 'd' || ReceiveBuffer[0] == 'x')
			{
				if(ReceiveBuffer[0] == 'd')
				{
					g_buff_type = 1;
				}
				else
				{
					g_buff_type = -1;
				}
				destroyAllWindows();
				break;
			}
			else if (ReceiveBuffer[0] == 'z')
			{
				Target.Mode = 1;
			}
			else if (ReceiveBuffer[0] == 's')
			{
				Target.Sentry = 1;
			}
			else if (ReceiveBuffer[0] == 'r'||ReceiveBuffer[0] == 'b')
			{
				gyro.Clear();
				Target.Mode = 0;
				Target.Sentry = 0;
			}
			clock_t start, end;
			start = clock();
			//cout << start <<endl;

#ifdef DEBUG		

			//通过键盘按键改变模式
			c = waitKey(10);

			if (c == 'd' || c == 'x')
			{
				if(c == 'd')
				{
					g_buff_type = 1;
				}
				else
				{
					g_buff_type = -1;
				}
				destroyAllWindows();
				break;
			}

			if (c == 'l')
			{
				Target.Mode = 1;
			}
			else if (c == 'k')
			{
				Target.Mode = 0;
				gyro.Clear();
			}
#endif

			if (ReceiveBuffer[0] == 'r')
			{
				g_color = 0;
				Target.color_of_enemy = 1;
				pthread_mutex_lock(&xmlfile_mutex);
#ifdef OPEN_SVM_PREDICT
				g_fs_read["svm_param"]["threshold_data"]["s_rgray_th"] >> Target.gray_th_;
				g_fs_read["svm_param"]["threshold_data"]["s_rcolor_th"] >> Target.color_th_;
#else
				g_fs_read["normal_param"]["threshold_data"]["rgray_th"] >> Target.gray_th_;
				g_fs_read["normal_param"]["threshold_data"]["rcolor_th"] >> Target.color_th_;
#endif
				pthread_mutex_unlock(&xmlfile_mutex);
			}
			else if (ReceiveBuffer[0] == 'b')
			{
				g_color = 1;
				Target.color_of_enemy = 0;
				pthread_mutex_lock(&xmlfile_mutex);
#ifdef OPEN_SVM_PREDICT
				g_fs_read["svm_param"]["threshold_data"]["s_bgray_th"] >> Target.gray_th_;
				g_fs_read["svm_param"]["threshold_data"]["s_bcolor_th"] >> Target.color_th_;
#else
				g_fs_read["normal_param"]["threshold_data"]["bgray_th"] >> Target.gray_th_;
				g_fs_read["normal_param"]["threshold_data"]["bcolor_th"] >> Target.color_th_;
#endif
				pthread_mutex_unlock(&xmlfile_mutex);
			}
			

			//获取图像
			pthread_mutex_lock(&srcimag_mutex); 

#ifdef OPEN_CAMERA
	cap >> srcimage;
#endif //OPEN_CAMERA

#ifdef DV_OPEN_CAMERA
	cap >> srcimage;
#endif //DV_OPEN_CAMERA

#ifdef KSJ_OPEN_CAMERA
	cap.getSrc(srcimage);
#endif //KSJ_OPEN_CAMERA

#ifdef DH_OPEN_CAMERA
	cap.getSrc(srcimage);
#endif //DH_OPEN_CAMERA
			
			pthread_mutex_unlock(&srcimag_mutex); 



			//开始图像处理
 			pthread_mutex_lock(&cope_mutex); 
			if (!srcimage.data)
			{
				cout << "Camera reading error!";
				pthread_mutex_unlock(&cope_mutex);
				continue;
			} 
		
			Roi_image = srcimage(RoiRect).clone();

			if (!Roi_image.data)
			{
				RoiRect = Rect(0, 0, GWIDTH, GHEIGHT);
				cout << "Roi_image reading error!";
				pthread_mutex_unlock(&cope_mutex);
				continue;
			}

			//图像处理主函数
			int cur_num = Target.TrackArmor(Roi_image, g_svm_load,  g_sentry_load, RoiRect, srcimage, gyro.armor_id_);

			//角度限制
			if (Target.yaw_ > 17 || Target.yaw_ < -17)
			{
				Target.Isfind = 0;
			}


			//判断是否找到装甲板
			if (Target.Isfind)
			{
				//判断是否是小陀螺模式
				if (Target.Mode)
				{
					int flag;

					flag = gyro.RoiGyro(Target.x_, Target.y_, Target.width_, Target.height_, cur_num, srcimage, RoiRect);

					RoiRect = Rect(gyro.left_x_, gyro.left_y_, gyro.right_x_ - gyro.left_x_, gyro.right_y_ - gyro.left_y_);

					Target.distance_ = compensation(vdistance, Target.distance_);
					unfind = 0;
					
					if (flag == 0)
					{
						uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 0);
						fire = 0;
					}
					else
					{
						fire++;
						if (fire < 2)
						{
							uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 1);//1 控制发弹
						}
						else
						{
							uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 0);
						}
					}
				}
				else
				{
					
	#ifdef DEBUG
	#else
					gyro.Clear();

					//找到装甲板就更新roi区域
					RoiRect = Rect(Target.x_, Target.y_, Target.width_, Target.height_);
	#endif
					Target.distance_ = compensation(vdistance, Target.distance_);
					unfind = 0;
					if (Target.angle_ > -2 && Target.angle_ < 2)
					{
						uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 2);//2 装甲板角度接近0
					}
					else
					{
						uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 0);
					}
				}

			}
			else
			{
				//有15帧未找到装甲板，则判定未找到装甲板 roi扩至原图
				unfind++;
				if (unfind >= 15)
				{
					gyro.Clear();
					RoiRect = Rect(0, 0, GWIDTH, GHEIGHT);
				} 

				// cout << "yaw :" << 0 << ", pitch :" << 0 << ", distance :" << 0 << endl;
				uart.Send(0, 0, 0, 9); //9 未找到
			} 
			pthread_mutex_unlock(&cope_mutex);


			//录制比赛视频
#ifdef OPEN_IMG_WRITE

				writeCount++;
				cout << writeCount << endl;
				//printf("writeCount=%d\n",writeCount);
				if ( writeCount <= 50000)//100 < writeCount &&
				{
					string Img_Name = "img/" + to_string(writeCount) + ".png";	
					imwrite(Img_Name, srcimage);
				}
				else if (writeCount > 50000)
				{
					writeCount = 51000;
				}		

#endif

			//帧率输出
			end = clock();
			double fps = 1 / ((double)(end - start) / CLOCKS_PER_SEC);
			 cout << "fps = " << fps << endl;
		}



		//能量机关部分
		energy_update();
		
		//以下变量不要改成全局变量
		Mat frame, image;

		int threshValue = 0;
		float angle = 0;

		buff_detect buff(g_color);
		get_omega GetOmega;
		logo center_r;
		SendData uart1;

		GetOmega.frame_center_y = GHEIGHT / 2;
		GetOmega.frame_center_x = GWIDTH / 2;
		pthread_mutex_unlock(&mutex1);//解锁

		while (1)
		{
			float yaw_n = 0;
			float pitch_n = 0;

#ifdef DEBUG
			c = waitKey(10);

			if (c == 'z')
			{
				destroyAllWindows();
				break;
			}
#else
			if (ReceiveBuffer[0] == 'z')
			{
				destroyAllWindows();
				break;
			}
#endif//DEBUG

		
			pthread_mutex_lock(&mutex1);//加锁
#ifdef OPEN_CAMERA
	cap >> frame;
#endif //OPEN_CAMERA

#ifdef DV_OPEN_CAMERA
	cap >> frame;
#endif //DV_OPEN_CAMERA

#ifdef KSJ_OPEN_CAMERA
	cap.getSrc(frame);
#endif //KSJ_OPEN_CAMERA

#ifdef DH_OPEN_CAMERA
	cap.getSrc(frame);
#endif //DH_OPEN_CAMERA
			pthread_mutex_unlock(&mutex1);
			

			if (frame.empty())
			{
				break;
			}
			frame.copyTo(image);

			//图像预处理
       		        buff.color_buff(image, g_color);

			//能量机关的识别		image 原图像    dst颜色通道相减二值图
			g_target_num = buff.BuffDetectTask(image, angle, center_r);

			if (statr)
			{
				pthread_mutex_lock(&mutex4);//加锁
				if (g_direction == 0 && angle != 0)
				{
					//获取旋转方向
					energy_direction(angle);
				}
				pthread_mutex_unlock(&mutex4);//解锁
				
				if(g_target_num != 0)
				{
					//预测下一点的位置
					GetOmega.energy_buff(g_direction, g_buff_type, g_show_num, angle, g_time1, center_r);
				}

				pthread_mutex_lock(&mutex2);//加锁
				if (g_target_num != 0)
				{
					//将位置信息转化为角度
					GetOmega.forecast(image, yaw_n, pitch_n, center_r, g_show_num);
				}
				pthread_mutex_unlock(&mutex2);//解锁

				pthread_mutex_lock(&mutex3);//加锁
				g_show_num++;
				pthread_mutex_unlock(&mutex3);//解锁
			}

			uart1.Send(yaw_n, pitch_n * -1, 7, 0);

#ifdef DEBUG
			imshow("效果", image);
#endif //DEBUG
		}
	}
}





void* Run2(void* arg)
{
	while(1)
	{
		ArmorRecognition Target(g_fs_read);
		SendData uart;
		Mat srcimage;
		Mat Roi_image;

		while(1)
		{
			
			if (ReceiveBuffer[0] == 'd' || ReceiveBuffer[0] == 'x')
			{
				if(ReceiveBuffer[0] == 'd')
				{
					g_buff_type = 1;
				}
				else
				{
					g_buff_type = -1;
				}
				destroyAllWindows();
				break;
			}
			else if (ReceiveBuffer[0] == 'z')
			{
				Target.Mode = 1;
			}
			else if (ReceiveBuffer[0] == 's')
			{
				Target.Sentry = 1;
			}
			else if (ReceiveBuffer[0] == 'r'||ReceiveBuffer[0] == 'b')
			{
				gyro.Clear();
				Target.Mode = 0;
				Target.Sentry = 0;
			}
			clock_t start, end;
			start = clock();

#ifdef DEBUG		
			c = waitKey(10);
			if (c == 'd' || c == 'x')
			{
				if(c == 'd')
				{
					g_buff_type = 1;
				}
				else
				{
					g_buff_type = -1;
				}
				destroyAllWindows();
				break;
			}

			if (c == 'l')
			{
				Target.Mode = 1;
			}
			else if (c == 'k')
			{
				Target.Mode = 0;
				gyro.Clear();
			}
#endif
			
			if (ReceiveBuffer[0] == 'r')
			{
				g_color = 0;
				Target.color_of_enemy = 1;
				pthread_mutex_lock(&xmlfile_mutex);
#ifdef OPEN_SVM_PREDICT
				g_fs_read["svm_param"]["threshold_data"]["s_rgray_th"] >> Target.gray_th_;
				g_fs_read["svm_param"]["threshold_data"]["s_rcolor_th"] >> Target.color_th_;
#else
				g_fs_read["normal_param"]["threshold_data"]["rgray_th"] >> Target.gray_th_;
				g_fs_read["normal_param"]["threshold_data"]["rcolor_th"] >> Target.color_th_;
#endif
				pthread_mutex_unlock(&xmlfile_mutex);
			}
			else if (ReceiveBuffer[0] == 'b')
			{
				g_color = 1;
				Target.color_of_enemy = 0;
				pthread_mutex_lock(&xmlfile_mutex);
#ifdef OPEN_SVM_PREDICT
				g_fs_read["svm_param"]["threshold_data"]["s_bgray_th"] >> Target.gray_th_;
				g_fs_read["svm_param"]["threshold_data"]["s_bcolor_th"] >> Target.color_th_;
#else
				g_fs_read["normal_param"]["threshold_data"]["bgray_th"] >> Target.gray_th_;
				g_fs_read["normal_param"]["threshold_data"]["bcolor_th"] >> Target.color_th_;
#endif
				pthread_mutex_unlock(&xmlfile_mutex);
			}
			
			pthread_mutex_lock(&srcimag_mutex);

#ifdef OPEN_CAMERA
	cap >> srcimage;
#endif //OPEN_CAMERA

#ifdef DV_OPEN_CAMERA
	cap >> srcimage;
#endif //DV_OPEN_CAMERA

#ifdef KSJ_OPEN_CAMERA
	cap.getSrc(srcimage);
#endif //KSJ_OPEN_CAMERA

#ifdef DH_OPEN_CAMERA
	cap.getSrc(srcimage);
#endif //DH_OPEN_CAMERA
			
			pthread_mutex_unlock(&srcimag_mutex); 

			pthread_mutex_lock(&cope_mutex);
			if (!srcimage.data)
			{
				cout << "Camera reading error!";
				pthread_mutex_unlock(&cope_mutex);
				continue;
			} 
		
			Roi_image = srcimage(RoiRect).clone();

			if (!Roi_image.data)
			{
				RoiRect = Rect(0, 0, GWIDTH, GHEIGHT);
				cout << "Roi_image reading error!";
				pthread_mutex_unlock(&cope_mutex);
				continue;
			}

			int cur_num = Target.TrackArmor(Roi_image, g_svm_load, g_sentry_load, RoiRect, srcimage, gyro.armor_id_);
			if (Target.yaw_ > 17 || Target.yaw_ < -17)
			{
				Target.Isfind = 0;
			}

			if (Target.Isfind)
			{
				if (Target.Mode)
				{
					int flag;//yes or no fire
					
					flag = gyro.RoiGyro(Target.x_, Target.y_, Target.width_, Target.height_, cur_num, srcimage, RoiRect);

					RoiRect = Rect(gyro.left_x_, gyro.left_y_, gyro.right_x_ - gyro.left_x_, gyro.right_y_ - gyro.left_y_);

					Target.distance_ = compensation(vdistance, Target.distance_);
					unfind = 0;
					
					if (flag == 0)
					{
						uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 0);
						fire = 0;
					}
					else
					{
						fire++;
						if (fire < 2)
						{
							uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 1);
						}
						else
						{
							uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 0);
						}
					}
				}
				else
				{
					
	#ifdef DEBUG
	#else
					gyro.Clear();
					RoiRect = Rect(Target.x_, Target.y_, Target.width_, Target.height_);
	#endif
					Target.distance_ = compensation(vdistance, Target.distance_);
					unfind = 0;
					if (Target.angle_ > -2 && Target.angle_ < 2)
					{
						uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 2);
					}
					else
					{
						uart.Send(Target.yaw_, -1 * Target.pitch_, Target.distance_, 0);
					}
				}
			}
			else
			{
				
				unfind++;
				if (unfind >= 15)
				{
					gyro.Clear();
					RoiRect = Rect(0, 0, GWIDTH, GHEIGHT);
				} 

				// cout << "yaw :" << 0 << ", pitch :" << 0 << ", distance :" << 0 << endl;
				uart.Send(0, 0, 0, 9);
			} 
			pthread_mutex_unlock(&cope_mutex);
			end = clock();
			double fps = 1 / ((double)(end - start) / CLOCKS_PER_SEC);
			  cout << "fps = " << fps << endl;
			

		}



		//能量机关部分
		energy_update();
		
		//以下变量不要改成全局变量
		Mat frame, image;

		float angle = 0;

		buff_detect buff(g_color);
		get_omega GetOmega;
		logo center_r;
		SendData uart1;

		while (1)
		{
			float yaw_n = 0;
			float pitch_n = 0;

			if (ReceiveBuffer[0] == 'z')
			{
				destroyAllWindows();
				break;
			}

			pthread_mutex_lock(&mutex1);//加锁	
#ifdef OPEN_CAMERA
	cap >> frame;
#endif //OPEN_CAMERA

#ifdef DV_OPEN_CAMERA
	cap >> frame;
#endif //DV_OPEN_CAMERA

#ifdef KSJ_OPEN_CAMERA
	cap.getSrc(frame);
#endif //KSJ_OPEN_CAMERA

#ifdef DH_OPEN_CAMERA
	cap.getSrc(frame);
#endif //DH_OPEN_CAMERA
			pthread_mutex_unlock(&mutex1);

			if (frame.empty())
			{
				break;
			}
			frame.copyTo(image);

			//图像预处理
			buff.color_buff(image, g_color);
			
			//能量机关的识别		image 原图像    dst颜色通道相减二值图
			g_target_num = buff.BuffDetectTask(image, angle, center_r);
			
			if (statr)
			{
				pthread_mutex_lock(&mutex4);//加锁
				if (g_direction == 0 && angle != 0)
				{
					energy_direction(angle);
				}
				pthread_mutex_unlock(&mutex4);//解锁
		
				if(g_target_num != 0)
				{
					//预测下一点的位置	
					GetOmega.energy_buff(g_direction, g_buff_type, g_show_num, angle, g_time1, center_r);
				}		
		
				pthread_mutex_lock(&mutex2);//加锁
				if (g_target_num != 0)
				{
					//将位置信息转化为角度
					GetOmega.forecast(image, yaw_n, pitch_n, center_r, g_show_num);
				}
				pthread_mutex_unlock(&mutex2);//解锁

				pthread_mutex_lock(&mutex3);//加锁
				g_show_num++;
				pthread_mutex_unlock(&mutex3);//解锁
			}

			uart1.Send(yaw_n, pitch_n * -1, 7, 0);
		}
	}
}




void ImgToVideo()
{
	VideoWriter video("./video/RM.avi", VideoWriter::fourcc('X', 'V', 'I', 'D'), 30, Size(GWIDTH, GHEIGHT));
	String img_path = "./img/";
	vector<String> img;
	int beginCount = 0;

	glob(img_path, img, false);

	size_t count = img.size();
	cout << "正在处理\n" << endl;
	for (size_t i = beginCount; i < count+ beginCount; i++)
	{
		stringstream str;
		str << i << ".png";
		Mat image = imread(img_path + str.str());
		if (!image.empty())
		{
			video << image;
			cout << "正在处理第" << i << "帧" << endl;
		}
	}
	cout << "处理完毕！" << endl;
}


void* show_time(void* arg)
{
	char com[20] = "/dev/TYY_USB";

	while (1)
	{
		
		SerialPort_Recv(ReceiveBuffer, 1);
		
		if (g_target_num != 0 && statr == false)
		{
			statr = true;
			g_time0 = static_cast<double>(getTickCount());//开始计时
		}

		if (g_num != g_show_num)
		{
			g_time0 = ((double)getTickCount() - g_time0) / getTickFrequency();//停止计时

			// cout << g_time0 << endl;
			g_time1 = g_time1 + g_time0;
			g_time0 = static_cast<double>(getTickCount());//重新开始计时
		}

		g_num = g_show_num;

		//waitKey(5);//由于while循环里循环太快，判断条件中的变量来不及更新，所以要延时
	}
}

void energy_direction(float angle)
{
	//判断方向
	if (g_show_num % 10 == 0 && g_angle1 == 0)
	{
		g_angle1 = angle;

		return;
	}

	if (g_show_num % 10 == 0 && g_direction == 0)
	{
		g_d_angle = angle - g_angle1;

		if (g_d_angle < -330 || (g_d_angle > 0 && g_d_angle < 30))
		{
			g_direction = 1;//顺

			cout << "顺" << endl;
		}
		else if (g_d_angle > 330 ||(g_d_angle < 0 && g_d_angle > -30))
		{
			g_direction = -1;//逆

			cout << "逆" << endl;
		}
		else
		{
			g_angle1 = 0;
		}
	}
}

//能量机关的参数更新
void energy_update()
{
	g_num = 0;
	g_show_num = 0;
	g_target_num = 0;
	g_direction = 0;

	g_time0 = 0, g_time1 = 0;
	g_angle1 = 0;
	g_d_angle = 0;

	statr = false;
}
